机器人力控混合拖动功能包调用介绍文档
| 修订日期 | 修订版本 | 修订内容 | 修订人 |
|---|---|---|---|
| 2024.8.23 | V1.0 | 初始化拖动功能包调用流程介绍 | 高振宇 |
| 2021.10.12 | V1.1 | 增加拖动功能包使用示例 | 钟一辰 |
[TOC]
本文档介绍机器人带传感器可指定拖动方向的力控混合拖动功能包(对标UR5e)调用流程。
在实现拖动功能前机器人固有参数(重力加速度、关节力矩系数、关节摩擦参数、本体和工具动力学参数等)和驱动层速度、电流的滤波参数设置必不可少，可参考文档。参数设置完成后调用以下两个接口:
1 使能拖动功能
/**
* @brief 使能拖动功能
* @param robot: 机器人指针
* @param enable: 拖动使能结构体，具体含义参见HandGudingEnable
* @return 返回值< 0 表示使能失败
*/
int FPHandGuidingEnable(const ARAL::interface::ARALIntfacePtr& robot, const HandGudingEnable& enable);
使能结构体参数说明:
struct HandGudingEnable
{
interface::RLJntArray q_init; // 初始关节位置
interface::RLJntArray qd_init; // 初始关节速度
interface::RLJntArray qdd_init; // 初始关节加速度
interface::ToolWorkpiece tool_workpiece; // 工具工件信息
std::vector<int> direciton; // 方向使能向量, 长度为工作空间维度6, 顺序为[x,y,z,Rx,Ry,Rz], 元素取值0(拖动关闭)或1(拖动开启)
interface::RLJntArray jnt_damp; // 拖动关节手感, 6个方向全开时生效, 取值范围[0, 1](值越大手感越沉) 默认值0.5
interface::DoubleVec end_omnidirectional_damp; // 拖动末端手感, 6个方向全开且末端有传感器时生效, 长度和顺序与方向使能向量一致, 取值范围和默认值与关节手感一致
interface::DoubleVec end_directional_damp; // 拖动末端手感, 非6个方向全开且末端有传感器时生效, 长度和顺序与方向使能向量一致, 取值范围和默认值与关节手感一致
interface::DoubleVec force_threshold; // 力响应阈值, 长度和顺序与方向使能向量一致
interface::DoubleVec force_limit; // 力响应极限, 长度和顺序与方向使能向量一致
}; // 拖动功能包使能结构体
- 开启拖动前，需把初始的关节PVA和工具工件信息传入使能接口
- 通过设置
direciton0 或 1，选择要进行拖动的方向 jnt_damp与软件侧阻尼进度条匹配end_omnidirectional_damp暂不需调整，实际暂未生效end_directional_damp暂不需调整，实际暂未生效force_limit为力响应最大值，当实际接触力大于最大值时将以设置的极限值响应，用户可根据实际应用场景调整force_threshold为力响应最小值，当实际接触力小于最小值时认为没有接触力，用户可根据实际应用场景调整
2 实时计算指令输出
/**
* @brief 更新拖动输出
* @param robot: 机器人指针
* @param update: 更新拖动输出结构体，具体含义参见HandGudingUpdate
* @return 返回值< 0 表示计算失败
*/
int FPHandGuidingUpdate(const ARAL::interface::ARALIntfacePtr& robot, const HandGudingUpdate& update, interface::JointCommand& out);
更新结构体参数说明:
struct HandGudingUpdate
{
RobotState state; // 机械臂状态
interface::RLPose task_frame; // 任务控制坐标系相对基坐标系位姿
interface::DescribeSpace space; // 任务空间描述类型
}; // 拖动功能包更新输出结构体
- 实时传入机械臂状态
task_frame暂不需设置，默认全为 0space预留参数暂不需设置，默认为笛卡尔空间
注意：使能结构体HandGudingEnable中力控方向向量direciton不全为1时，取JointCommand中位置指令，运行在位置环；全为1时取力矩指令，运行在电流环
3 功能包使用实例
//! 机器人初始化（算法内部接口，无需参考）
Setup("aubo_i5");
//! 仿真数据读取
std::vector<std::vector<double>> data;
ReadFromFile2D("./test/data/control/fd_feat_pack_new2.csv", ",", data);
unsigned int N = data.size();
//! 设置传感器位姿
interface::RLPose sensor_pose = {0.0, 0.0, 0.047, 0.0, 0.0, 0.0};
robot->mdlSetEndSensorPoseInFlange(sensor_pose);
//! 设置tcp位姿
interface::RLPose TCP_pose = {0.0, 0.0, 0.130, 0.0, 0.0, 0.0};
robot->mdlSetToolPose(TCP_pose);
//! 负载辨识（如果有精确CAD参数，可跳过这步）
std::vector<interface::RLWrench> calib_measure = {[19.3476,26.6873,8.05786,2.49509,-1.07469,0.0467071], [27.1562,35.3499,8.32085,1.79869,-0.432925,0.0399273], [19.8203,32.7533,15.1771,1.93428,-1.10004,0.0469478]};
std::vector<interface::RLPose> calib_pose = {[-0.261797, 0.261797, 1.309, 1.0472, 1.39625, 0.0], [-0.628319, 0.471245, 1.65806, -0.471239, -7.33606e-06, -3.66803e-06], [-0.628315, 0.0665102, 1.34533, -0.104715, 1.57081, 3.66803e-06]};
interface::RLWrench sensor_offset;
interface::ToolCalibResult calibresult;
robot->calibToolDynamicParameterWithFTSensor(calib_pose, calib_measure, calibresult, sensor_offset)
//! 设置工具动力学参数
ARAL::interface::RLInertia tool;
tool.mass = 0.539328;
tool.center = {-0.000196702,-0.0013368,0.0689383};
tool.inertia.fill(0.);
robot->mdlSetLoadDynamicParameterInFlange(tool);
//! 设置摩擦力参数
std::vector<interface::FrictionParam> friction_para(6);
std::vector<double> Fs = {1.2, 1.2, 1.2, 0.3, 0.445668, 0.340624};
std::vector<double> Fc = {0.77709, 0.970387, 0.970387, 0.205978, 0.299995, 0.24172};
std::vector<double> Vs = {0.01, 0.01, 0.01, 0.01, 0.199999, 0.2};
std::vector<double> Miu = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
std::vector<double> Fv0 = {0.0222653, 0.0375545, 0.0375545, 0.00107317, 0.00949945, 0.000145128};
std::vector<double> Fv1 = {1.5, 1.25455, 1.25455, 0.279202, 0.336642, 0.307392};
std::vector<double> Fv2 = {0.4954, 0.369846, 0.369846, 0.0295175, 0.0466343, 0.0326262};
std::vector<double> Fv3 = {0.0776088, 0.0542497, 0.0542497, 0.000424173, 0.00192163, 0.0};
std::vector<double> Ft1 = {1.0, 1.0, 1.0, 0.396167, 0.477587, 0.5};
std::vector<double> Ft2 = {-0.0241953, -0.0243861, -0.0243861, -0.00838675, -0.0101449, -0.0104789};
std::vector<double> Ft3 = {4000.0, 4000.0, 4000.0, 999.999, 1000.0, 1000.0};
std::vector<double> c1 = {0.001491, 0.001491, 0.001491, 0.01002, 0.01002, 0.01002};
std::vector<double> c2 = {-0.006469, -0.006469, -0.006469, 0.005259, 0.005259, 0.005259};
for(unsigned int i = 0; i < dof; i++)
{
friction_para[i].type = interface::FrictionType::VTL_Model;
friction_para[i].para.resize(13);
friction_para[i].para = { Fs[i], Fc[i], Vs[i], Miu[i], Fv0[i], Fv1[i], Fv2[i], Fv3[i], Ft1[i], Ft2[i], Ft3[i], c1[i], c2[i] };
}
robot->mdlSetJointFrictionParameter(friction_para);
//! 设置本体动力学模型
std::vector<double> dynamic_para = {0.0,
4.848581, -0.000048, 0.008761, -0.067172, 0.012788, 0.000000, -0.000005, 0.012731, 0.000224, 0.008119,
10.836261, 2.210597, 0.000000, 0.071552, 0.026496, -0.000020, -0.014597, 0.884239, 0.000000, 0.876117,
2.849168, 0.753964, -0.000057, 0.266713, 0.028601, 0.000008, -0.070747, 0.289813, 0.000003, 0.264194,
1.628022, 0.000062, 0.018361, -0.002506, 0.002162, -0.000004, -0.000001, 0.001158, 0.000070, 0.002138,
1.628022, -0.000062, -0.018361, -0.002506, 0.002162, -0.000004, 0.000001, 0.001158, -0.000070, 0.002138,
0.197800, 0.000004, 0.000119, -0.003379, 0.000176, -0.000000, 0.000000, 0.000170, 0.000001, 0.000184};
robot->mdlSetRobotLinkDynamicParameter(dynamic_para);
//! 设置力矩系数
interface::RLJntArray joint_constant = {8.72,8.72,8.72,7.092,7.092,7.092};
robot->mdlSetJointTorqueConstant(joint_constant);
//! 开启拖动
int ret;
ARAL::pack::HandGudingEnable fd_init;
ARAL::pack::HandGudingUpdate fd;
interface::JointCommand cmd_out;
fd_init.direciton.resize(dof);
fd_init.end_omnidirectional_damp.resize(dof);
fd_init.end_directional_damp.resize(dof);
fd_init.force_limit.resize(dof);
fd_init.force_threshold.resize(dof);
fd_init.q_init = {0.0, 0.0, 1.57, 0.0, 1.57, 0.0};
fd_init.qd_init = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
fd_init.qdd_init = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
fd_init.direciton = {1, 1, 1, 1, 1, 1};
fd_init.jnt_damp = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5}; // 方向全开时设置
fd_init.end_omnidirectional_damp = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0}; // 方向全开，且末端有传感器时设置
fd_init.end_directional_damp = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5}; // 方向未全开时设置
fd_init.force_limit = {100.0, 100.0, 100.0, 10.0, 10.0, 10.0};
fd_init.force_threshold = {2.0, 2.0, 2.0, 0.5, 0.5, 0.5};
//! 调用拖动功能包使能接口
if (ARAL::pack::FPHandGuidingEnable(robot, fd_init) < 0)
{
LOGE(log_) << "拖动功能包使能失败" << ENDL(log_);
}
//! 周期性更新控制参数和计算结果
ARAL::interface::RLWrench sensor_wrench;
ARAL::interface::RLJntArray q_cur, qd_cur, qdd_cur;
ARAL::interface::RLJntArray temp, real_torq, fric_torq;
size_t i = 0;
while (i < N)
{
for(unsigned int j = 0; j < dof; j++)
{
q_cur[j] = data[i][j + 1 + dof * 0];
qd_cur[j] = data[i][j + 1 + dof * 1];
qdd_cur[j] = data[i][j + 1 + dof * 2];
real_torq[j] = data[i][j + 1 + dof * 3];
temp[j] = data[i][j + 1 + dof * 4];
fric_torq[j] = data[i][j + 1 + dof * 5];
sensor_wrench[j] = data[i][j + 1 + dof * 6];
}
fd.state.q = q_cur;
fd.state.qd = qd_cur;
fd.state.qdd= qdd_cur;
fd.state.temp = temp;
fd.state.torq = real_torq;
fd.state.friction = fric_torq;
fd.space = ARAL::interface::DescribeSpace::CARTESIAN;
fd.task_frame = ARAL::interface::RLPose();
fd.state.sensor = sensor_wrench; //该结构体的传感器数据需减去偏置（sensor_offset）
//! 计算指令力矩/位置
ret = ARAL::pack::FPHandGuidingUpdate(robot, fd, cmd_out);
i++;
}